/**
 * @file robot_driver_base.h
 * @brief 定义控制机器人运动的和与机器人平台交互的接口
 * @author weeksun
 * @version 1.0
 * @date 2017-09-30
 */
#ifndef ROBOT_DRIVER_BASE_H
#define ROBOT_DRIVER_BASE_H

#include <string>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <nav_msgs/Odometry.h>
#include <vector>
#include <geometry_msgs/Point.h>
#include <tf/tf.h>
#include <yaml-cpp/yaml.h>
#include <yaml-cpp/node/detail/node.h>

#include <boost/asio.hpp>
#include <boost/thread.hpp>
#include <time.h>
#include <iomanip>

/**
 * @brief 包含AGV机器人执行特定任务的相关代码
 */
namespace AgvAssistant {


class RobotDriverBase;

/**
 * @brief 工作区
 */
struct WorkArea{
  WorkArea():
    seq(0){}

  int buc_statue;
  std::string task;
  int seq;   ///< 工作区编号
  int type;  ///< 工作区类型
  tf::Vector3 dock;   ///< 工作区定位点
  std::vector<tf::Vector3> area; ///< 工作区连接点，逆时针连接
  double angle;
};

typedef boost::shared_ptr<RobotDriverBase> RobotDriverBasePtr;   ///< RobotDriverBase共享指针类型

/**
 * @brief 该类定义了机器人与特定任务之间交互的接口
 */
class RobotDriverBase
{
public:
  /**
   * @brief 位姿调整响应信息
   */
  struct PoseAckInfo{
    PoseAckInfo():
      ack_code(0),
      success(0),
      task_name(""),
      task_stamp(0){}

    int ack_code;    ///< 响应代码
    int success;
    unsigned int task_stamp;   ///< 任务时间戳
    std::string task_name;    ///< 任务名称
  };

  struct RobotStatusInfo{
    std::string robot_id;    ///< 机器人ID
    bool is_online;       ///< 群控连接情况
    int mode;           ///< 运行模式
    bool is_relocated;   ///< 是否重定位
    unsigned int map_id;        ///< 所处地图id
    float pos_x;        ///< 机器人坐标
    float pos_y;        ///< 机器人坐标
    float pos_theta;    ///< 机器人坐标
    std::string task_name;     ///< 当前任务
    int task_progress;  ///< 任务进度
    bool is_charging;     ///< 是否在充电
    int battery;        ///< 电量
    bool emergency;       ///< 是否急停
    int error_code;     ///< 任务错误代码
  };


  /**
   * @brief 动作类型枚举
   */
  enum SubTaskType{
    SUB_TASK_GO_TO_POINT,
    SUB_TASK_GO_ALONG_PATH,
    SUB_TASK_WAIT_MS,
    SUB_TASK_WAIT_MSG,
    SUB_TASK_SEND_MSG,
    SUB_TASK_SELF_DEF,
  };

  enum SubTaskAckStatus{
    SUB_TASK_ACK_BREAK=-2,
    SUB_TASK_ACK_EXCEPTION, ///< 自定义子任务执行成功
    SUB_TASK_ACK_ONGOING,  ///< 自定义子任务执行异常，不影响群控执行后续任务
    SUB_TASK_ACK_OK       ///< 自定义子任务执行失败，后续任务无法执行，需手动清除群控错误
  };

  /**
   * @brief 群控信息
   */
  struct GroupControlInfo{
    GroupControlInfo():
      request(0),
      type(0),
      evt_str(""),
      task_name(""),
      task_stamp(0),
      status(SUB_TASK_ACK_OK){}

    int request;   ///< 请求代号
    int type;      ///< 任务类型
    int data;      ///< 数据
    std::string evt_str;   ///< 事件标识字符串
    unsigned int task_stamp;  ///< 任务时间戳
    std::string task_name;    ///< 任务名称
    SubTaskAckStatus status;  ///< 子任务执行状态

  };

  /**
   * @brief 应急信息
   */
  struct EmergentInfo{
    EmergentInfo():
      cmd(0){}

    int cmd;      ///< 命令类型
  };

  /**
   * @brief 串口通信命令
   */
  enum OrderClass{
    PREPARECLOSE,
    SENDCLOSE,
    STATUESIGAL,
    SENDEXPEND,
    STOPWORK
  };

  /**
   * @brief 串口通信命令类型
   */
  enum OrderType{
    CONTROL,
    STATUEINQUIRE,
    ERRORINQUIRE
  };

  typedef boost::function<void(const WorkArea& area)> WorkAreaCallback; ///< 工作区设置回调函数
  typedef boost::function<void(const EmergentInfo& info)> EmergentCallback; ///< 应急请求回调函数
  typedef boost::function<void(const GroupControlInfo& ctl)> GroupControlCallback; ///< 群控回调函数
  typedef boost::function<void(const PoseAckInfo& ack)> PoseAdjustAckCallback;  ///< 位姿调整响应回调函数
  typedef boost::function<void(const nav_msgs::OdometryConstPtr& msg)> RobotOdomCallback;  ///< 轮式里程计回调函数
  typedef boost::function<void(const geometry_msgs::Pose& pose)> WorldPoseCallback; ///< 机器人世界坐标系位姿获取回调函数
  typedef boost::function<void(const std::vector<tf::Vector3>& pose)> RobotLaserPoseCallback; ///< 激光世界坐标系位姿获取回调函数
  typedef boost::function<void(const RobotStatusInfo& status)> RobotStatusInfoCallback;  ///< 机器人状态回调函数

  /**
   * @brief 创建机器人驱动基类对象，该类不能直接创建对象，而是由子类构造函数调用
   * @param driver_name [in] 驱动名称，作为参数读取时的图元名称前缀
   */
  RobotDriverBase(std::string driver_name):driver_name_(driver_name) {;}
  /**
   * @brief 发布速度控制命令,该函数由子类根据不同机器人实现
   * @param x [in] 机器人X方向的速度(单位:m/s)，X轴正向一般为机器人正前方
   * @param y [in] 机器人Y方向的速度(单位:m/s)，Y轴正方向一般为垂直于机器人X方向向左
   * @param vth [in] 机器人旋转角速度(单位:rad/s),机器人逆时针旋转为正
   * @return 成功发布该命令返回true
   */
  virtual bool pubVel(float x,float y,float vth){return false;}
  /**
   * @brief 发送机器人位姿调整请求，该函数有子类根据不同机器人实现
   *
   *  该函数发送某一位置相对当前位置的相对偏移来调整机器人位姿，当调整状态将通过'poseAck'消息反馈
   * @param request_code [in]
   *  1-基于里程计，借助底盘原始数据达到位置调节的功能，遥控和巡航模式通用
   *  2-基于SLAM，借助修正后的世界坐标调节机器人位置，一般用于巡航模式
   * @param switch_x [in] 机器人x，即前后方向是否做调整 是-true 否-false,该选项不一定对所有机器人都有效
   * @param switch_y [in] 机器人y，即左右方向是否做调整 是-true 否-false,该选项不一定对所有机器人都有效
   * @param switch_th [in] 机器人z，即角度是否做调整 是-true 否-false,该选项不一定对所有机器人都有效
   * @param x_set [in] x轴相对移动量(单位：m) +相对当前位置前进 -相对当前位置后退
   * @param y_set [in] y轴相对移动量(单位：m) +相对当前位置左平移 -相对当前位置右平移
   * @param th_set [in] z轴相对旋转量(单位：rad) +相对当前位置逆时针旋转 -相对当前位置顺时针旋转
   * @param max_vel_x [in] x轴调整最大线速度(单位：m/s),正值,该选项不一定对所有机器人都有效
   * @param max_vel_y [in] y轴调整最大线速度(单位：m/s),正值,该选项不一定对所有机器人都有效
   * @param max_vel_th [in] z轴调整最大角速度(单位：rad/s),正值,该选项不一定对所有机器人都有效
   * @param precision_x [in] x轴调节精度(单位：m),正值,该选项不一定对所有机器人都有效
   * @param precision_y [in] y轴调节精度(单位：m),正值,该选项不一定对所有机器人都有效
   * @param precision_th [in] z轴调节精度(单位：rad),正值,该选项不一定对所有机器人都有效
   * @return 成功发布请求放回true
   */
  virtual bool sendPoseAdjustRequest(int request_code,
                                     bool switch_x,bool switch_y,bool switch_th,
                                     double x_set,double y_set,double th_set,
                                     double max_vel_x,double max_vel_y,double max_vel_th,
                                     double precision_x,double precision_y,double precision_th){
    return false;
  }
  /**
   * @brief 发送群控响应信息
   *
   *  当执行自定义群控任务后必须通过该函数返回对应任务的事件标识字符串
   * @param ack [in] 群控信息
   * @return 成功发送信息返回true
   */
  virtual bool sendGroupControlAck(const GroupControlInfo& ack){
    return false;
  }

  /**
   * @brief 向串口发送指令
   * @param order[in] 指令
   * @param size[in] 字节数
   * @param type 类型
   * @return 成功发送
   */
  virtual bool armSend(char order[],int size,int type){
    return false;
  }

  /**
   * @brief 发送复位指令
   * @param order[in] 指令
   * @param size[in] 字节数
   * @return 成功发送
   */
  virtual bool armSendReset(char order[],int size){
    return false;
  }

  virtual bool serialCommunication(OrderClass order){

  }

  /**
   * @brief 注册Odom数据获取回调函数
   * @param callback [in] 绑定函数对象，请使用boost::bind对函数进行绑定
   */
  void registerOdomCallback(const RobotOdomCallback& callback){
    odom_callback_ = callback;
  }
  /**
   * @brief 注册机器人世界坐标系位姿获取回调函数
   * @param callback [in] 回调函数对象，请使用boost::bind对函数进行绑定
   */
  void registerWorldPoseCallback(const WorldPoseCallback& callback){
    world_pose_callback_ = callback;
  }


  /**
   * @brief 注册激光数据在机器人坐标系位姿获取回调函数
   * @param callback [in] 回调函数对象，请使用boost::bind对函数进行绑定
   */
  void registerRobotLaserPoseCallback(const RobotLaserPoseCallback& callback){
    robot_laser_pose_callback_ = callback;
  }

  /**
   * @brief 注册位姿调整响应回调函数
   *
   *  当外部调用sendPoseAdjustRequest函数成功，并且使用该函数注册了回调，位姿调整响应信息将通过回调函数返回到外部
   * @param callback [in] 回调函数对象，请使用boost::bind对函数进行绑定
   */
  void registerPoseAdjustAckCallback(const PoseAdjustAckCallback& callback){
    pose_adjust_ack_callback_ = callback;
  }

  /**
   * @brief 注册群控回调函数
   *
   *  群控回调函数用于请求执行特定控制程序或自定义任务，具体执行某项任务由群控信息决定
   * @param callback [in] 回调函数对象，请使用boost::bind对函数进行绑定
   */
  void registerGroupControlCallback(const GroupControlCallback& callback){
    group_control_callback_ = callback;
  }
  /**
   * @brief 注册机器人状态信息回调函数
   * @param callback [in] 回调函数对象，请使用boost::bind对函数进行绑定
   */
  void registerRobotStatusInfoCallback(const RobotStatusInfoCallback& callback){
    robot_status_info_callback_ = callback;
  }

  /**
   * @brief 注册应急信息回调函数
   *
   *  用户接收到应急信息后执行自己的安全控制代码
   *    - EmergentInfo.cmd  0-取消任务 终止
   *    - EmergentInfo.cmd  1-中止任务 暂停
   *    - EmergentInfo.cmd  2-恢复任务 继续
   * @param callback [in] 回调函数对象，请使用boost::bind对函数进行绑定
   */
  void registerEmergentCallback(const EmergentCallback& callback){
    emergent_callback_ = callback;
  }

  void registerWorkAreaCallback(const WorkAreaCallback& callback){
    work_area_callbakc_ = callback;
  }

  void test(){
    if(!odom_callback_.empty()){
      nav_msgs::OdometryPtr ptr = nav_msgs::OdometryPtr(new nav_msgs::Odometry());
      ptr->header.frame_id = "test";
      odom_callback_(ptr);
    }
  }

  void text_yaml(std::string yaml_num,std::string& task_type,WorkArea& work_area)
  {
     YAML::Node node = YAML::Load(yaml_num);
     task_type = node["task"].as<std::string>();
     work_area.seq= node["id"].as<int>();
     work_area.type = node["type"].as<int>();
     tf::Vector3 p1;
     p1.setX(node["p1"][0].as<double>());
     p1.setY(node["p1"][1].as<double>());
     p1.setZ(0.0);
     tf::Vector3 p2;
     p2.setX(node["p2"][0].as<double>());
     p2.setY(node["p2"][1].as<double>());
     p2.setZ(0.0);
     tf::Vector3 p3;
     p3.setX(node["p3"][0].as<double>());
     p3.setY(node["p3"][1].as<double>());
     p3.setZ(0.0);
     tf::Vector3 p4;
     p4.setX(node["p4"][0].as<double>());
     p4.setY(node["p4"][1].as<double>());
     p4.setZ(0.0);
     tf::Vector3 p5;
     p5.setX(node["p5"][0].as<double>());
     p5.setY(node["p5"][1].as<double>());
     p5.setZ(0.0);

     work_area.dock = p5;
     work_area.area.push_back(p1);
     work_area.area.push_back(p2);
     work_area.area.push_back(p3);
     work_area.area.push_back(p4);
  }
  unsigned char data_flag_[4];

protected:
  std::string driver_name_;     ///< 机器人驱动名称，在参数读取时作为参数图元名称的前缀
  RobotOdomCallback odom_callback_;  ///< 机器人轮式里程计回调函数对象,该函数对象在子类中调用
  WorldPoseCallback world_pose_callback_; ///< 机器人世界坐标系位姿回调函数对象，该函数对象在子类中调用
  RobotLaserPoseCallback robot_laser_pose_callback_; ///< 激光数据机器人坐标系位姿回调函数对象，该函数对象在子类中调用
  PoseAdjustAckCallback pose_adjust_ack_callback_; ///< 位姿调整响应回调函数对象，该函数对象在子类中调用
  GroupControlCallback group_control_callback_;  ///< 群控回调函数对象，该函数对象在子类中调用
  EmergentCallback emergent_callback_;    ///< 应急请求回调函数对象，该函数对象在子类中调用
  WorkAreaCallback work_area_callbakc_;   ///< 工作区设置回调函数对象，该函数对象在子类中调用
  RobotStatusInfoCallback robot_status_info_callback_;  ///< 机器人状态信息回调函数对象，该函数对象在子类中调用

  unsigned char write_data_[10];
  unsigned char read_data_[9];
  bool begin_write_;
  bool recv_data_;

  boost::thread arm_working_th_;     ///< 任务线程对象
  bool arm_working_;        ///< 任务线程结束标志

};


}

#endif // ROBOT_DRIVER_BASE_H
